Mapping and localization #21
Conversation
…ever, there is proper boradcasting of the odom frame, so the slam tool box should be really easy to implement. next push, make wheel odom better, filter odom with ekf by using imu data with robot_localizaiton package, have launch files for mappingand localization
…one lap around track it was oof the starting by like a few meters, next will be to get mapping and localization working
… wrong with the matrix math, and ending up with nan as vals for state
… to work better, also for some reason if i delete some log statements, it pushes nan vals. I honestly don't know why observing the numbers makes it work. plz send help
…eck after how many iteration it dies
hepromark
left a comment
hepromark
left a comment
There was a problem hiding this comment.
Really good first draft!
- Some nits
- Add tests
- Some design stuff but not much
| right_encoder_curret = right_encoder_data->position[0]; | ||
| left_encoder_current = left_encoder_data->position[0]; | ||
|
|
||
| double right_delta = right_encoder_curret - right_encoder_last; |
| double angular_velocity = 0.0; | ||
|
|
||
|
|
||
| if (std::abs(current_steering) < 1e-4) { |
There was a problem hiding this comment.
nit: make this const param
| right_encoder_last = right_encoder_curret; | ||
| left_encoder_last = left_encoder_current; | ||
|
|
||
| //RCLCPP_INFO(this->get_logger(),"x = %f, y = %f, yaw = %f, v = %f , av = %f", x , y, yaw, velocity, angular_velocity); |
There was a problem hiding this comment.
| //RCLCPP_INFO(this->get_logger(),"x = %f, y = %f, yaw = %f, v = %f , av = %f", x , y, yaw, velocity, angular_velocity); |
| if (current_steering > STEERING_NORMAL) current_steering = STEERING_NORMAL; | ||
| if (current_steering < -STEERING_NORMAL) current_steering = -STEERING_NORMAL; |
There was a problem hiding this comment.
If you're pulling this from the sim, when would this be above the max? Or is the clamping a bicycle model parameter?
| //set the translations | ||
| t.transform.translation.x = x; | ||
| t.transform.translation.y = y; | ||
| t.transform.translation.z = 0.0592; |
There was a problem hiding this comment.
Not sure where this Z is coming from- if its sim param make it const in the class
| the simulation relative to the world frame | ||
| (determined using the ips and intutions) | ||
| */ | ||
| x = 0.7412, y = 3.1583, yaw = -M_PI/2,left_encoder_last = 0,right_encoder_last = 0,right_encoder_curret = 0,left_encoder_current = 0; |
There was a problem hiding this comment.
Q: Would this just be 0 on the real car? x, y, and yaw that is? If it is, make it a parameter.
Ideally, once we have both sim & real software stacks, we'll expose a parameter file that all the classes grab initialization / constants from
There was a problem hiding this comment.
ya, I was planning on that too. the values of x y z and yaw are form the sim. they are the starting values relative to the world frame.
There was a problem hiding this comment.
Theres some pretty complex functions in this file. For stuff like ekf and obscreator (where the return value or variables being mutated) aren't clear, can you add:
/**
* @brief Computes the sum of two integers.
*
* @param a The first integer.
* @param b The second integer.
* @return The sum of a and b.
*/
int add(a,b) {...}
documentation style? It helps make the code much more readable. Especially in cases where you don't return and just mutate class variables (ex: wheel_odom functions), it makes debuging way way easier later. Thanks!
There was a problem hiding this comment.
Also add tests!
Don't have to be unit tests, but functional
- if input data is x then output is y
- You can generate y from a given x by ex: backwards deriving it from your dynamics model. At steering = 0.1 rad & constant accel, what is the expected odom after 1s? Then run that through the ekf w/ mock wheel odom and sensor data.
| intalized_time = true; | ||
|
|
||
| //initalize mu | ||
|
|
There was a problem hiding this comment.
nit: stay consistent, usualy you don't have a space between comments and code. So don't have a space here
| } | ||
|
|
||
| //for the break down how how this works look at the read me in package folder | ||
| void EKF_NODE::ekf(const nav_msgs::msg::Odometry &odom) { |
There was a problem hiding this comment.
nit: Make this more descriptive. Something like "ekf_pass"
| t_previous = t_current; | ||
|
|
||
|
|
||
| //update state with ekf |
There was a problem hiding this comment.
My understanding of the architecture is that:
- wheel odom pub a message to
wheel_odom - EKF grabs these messages and computes a better
odomestimation
I think theres some design choices we can make on how the pub / sub sync works. Ex: should we always have these 2 synchronized, or should we desync them?
ping me in disc when u see this ill explain there
|
For the follow up PR, can you look into ROS params / some way of selecting a set of constants when launching the nodes? We ideally have 2 launch types, 1 for sim and 1 for real, so we don't have to edit src code every time we change modes. |
…as good as expected form the 7d version. however the performance and behaviour makes sense. the elf x and y closely track the odom x and y as the odom is the only source of x and y sensor data the filter has. also gamepad update is fully working and ready to use, the velocity estimate from ekf is pretty accurate and can be now used for e-break.
…o get better results
…e, was not building without it
| 1. Configure the production repository: | ||
| ``` | ||
| curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \ | ||
| && curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \ | ||
| sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \ | ||
| sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list | ||
| ``` | ||
|
|
||
| 2. Update the packages list from the repository : | ||
| ``` | ||
| sudo apt-get update | ||
| ``` | ||
| 3. Install the NVIDIA Container Toolkit packages | ||
| ``` | ||
| export NVIDIA_CONTAINER_TOOLKIT_VERSION=1.17.8-1 | ||
| sudo apt-get install -y \ | ||
| nvidia-container-toolkit=${NVIDIA_CONTAINER_TOOLKIT_VERSION} \ | ||
| nvidia-container-toolkit-base=${NVIDIA_CONTAINER_TOOLKIT_VERSION} \ | ||
| libnvidia-container-tools=${NVIDIA_CONTAINER_TOOLKIT_VERSION} \ | ||
| libnvidia-container1=${NVIDIA_CONTAINER_TOOLKIT_VERSION} | ||
| ``` | ||
| 5. Configure the container runtime by using the ```nvidia-ctk``` command | ||
| ``` | ||
| sudo nvidia-ctk runtime configure --runtime=docker |
There was a problem hiding this comment.
Q: Is this just installing nvidia-ctk? Or is it installing a special version of the toolkit? because I already have one on linux and not sure if I need these steps or not (and if I do double install if something is going to die)
There was a problem hiding this comment.
no its just the regular container toolkit form NVIDIA. I just put the instructions there, as the official docs don't give you all the steps. if you have a version installed, where you can access your GPU in the container, you don't need it. it would be nice though, to test out, if pf works without a NVIDIA GPU. also I wonder if you can even boot up into the container with the Nvidia specific changes I made to the compose file, if you won't have a Nvidia gpu. I think I will probably ask one of the members during the sync to try running it to see what happens.
…line, but explode when steering input is introduced
…s. there is something wrong with the timing of events which causes calculations to corrupt when there is not log statemnets. adding the log statemnet creates a tiny delay, thus fixing the issue. that is just my running hypothesei.will need to investigate furthur in the next term
Made a PR for easier code review. Please update this title & description before merging